/*! 
 * Implementation file for the collision detector.
 *
 * \author Devis Rossini
 * \date 20 March 2012
 */
#include "PhxCollisionDetector.h"

#include "PhxContacts.h"
#include "PhxIntersections.h"

#include "PhxUtils.h"

#include "PhxTriangle.h"

#include <memory>
#include <cassert>

using namespace phx;

// ---------- WRAPPER FUNCTIONS --------------------------------------------------------------------------------------------------------------------------

// Sphere - Sphere.
unsigned CollisionDetector::SphereAndSphere(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	const CollisionSphere* sphereOne = static_cast<const CollisionSphere*>(one);
	const CollisionSphere* sphereTwo = static_cast<const CollisionSphere*>(two);

	return SphereAndSphere(*sphereOne, *sphereTwo, data);
}

// Sphere - Plane.
unsigned CollisionDetector::SphereAndPlane(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	const CollisionSphere* sphere = static_cast<const CollisionSphere*>(one);
	const CollisionPlane* plane	= static_cast<const CollisionPlane*>(two);

	return SphereAndPlane(*sphere, *plane, data);
}

// Sphere - Terrain.
unsigned CollisionDetector::SphereAndTerrain(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	const CollisionSphere* sphere = static_cast<const CollisionSphere*>(one);
	const CollisionTerrain*	terrain	= static_cast<const CollisionTerrain*>(two);

	return TerrainAndSphere(*terrain, *sphere, data);
}

// Sphere - Box.
unsigned CollisionDetector::SphereAndBox(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	const CollisionSphere* sphere = static_cast<const CollisionSphere*>(one);
	const CollisionBox*	box	= static_cast<const CollisionBox*>(two);

	return BoxAndSphere(*box, *sphere, data);
}

// Plane - Sphere.
unsigned CollisionDetector::PlaneAndSphere(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	const CollisionPlane* plane	= static_cast<const CollisionPlane*>(one);
	const CollisionSphere* sphere = static_cast<const CollisionSphere*>(two);

	return SphereAndPlane(*sphere, *plane, data);
}

// Plane - Plane.
unsigned CollisionDetector::PlaneAndPlane(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	// Unimplemented.
	return 0;
}

// Plane - Terrain.
unsigned CollisionDetector::PlaneAndTerrain(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	// Unimplemented.
	return 0;
}

// Plane - Box.
unsigned CollisionDetector::PlaneAndBox(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	const CollisionPlane* plane = static_cast<const CollisionPlane*>(one);
	const CollisionBox*	box	= static_cast<const CollisionBox*>(two);

	return BoxAndPlane(*box, *plane, data);
}

// Terrain - Sphere.
unsigned CollisionDetector::TerrainAndSphere(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	const CollisionTerrain* terrain = static_cast<const CollisionTerrain*>(one);
	const CollisionSphere* sphere = static_cast<const CollisionSphere*>(two);

	return TerrainAndSphere(*terrain, *sphere, data);
}

// Terrain - Plane.
unsigned CollisionDetector::TerrainAndPlane(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	// Unimplemented.
	return 0;
}

unsigned CollisionDetector::TerrainAndTerrain(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	// Unimplemented.
	return 0;
}

// Terrain - Box.
unsigned CollisionDetector::TerrainAndBox(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	// Unimplemented.
	return 0;
}

// Box - Sphere.
unsigned CollisionDetector::BoxAndSphere(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	const CollisionBox*	box	= static_cast<const CollisionBox*>(one);
	const CollisionSphere* sphere = static_cast<const CollisionSphere*>(two);

	return BoxAndSphere(*box, *sphere, data);
}

// Box - Plane.
unsigned CollisionDetector::BoxAndPlane(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	const CollisionBox*	box	= static_cast<const CollisionBox*>(one);
	const CollisionPlane* plane = static_cast<const CollisionPlane*>(two);

	return BoxAndPlane(*box, *plane, data);
}

// Box - Terrain.
unsigned CollisionDetector::BoxAndTerrain(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	// Unimplemented.
	return 0;
}

// Box - Box.
unsigned CollisionDetector::BoxAndBox(const CollisionPrimitive* one, const CollisionPrimitive* two, CollisionData* data)
{
	const CollisionBox* boxOne = static_cast<const CollisionBox*>(one);
	const CollisionBox* boxTwo = static_cast<const CollisionBox*>(two);

	return BoxAndBox(*boxOne, *boxTwo, data);
}

unsigned CollisionDetector::SphereAndSphere(const CollisionSphere& one, const CollisionSphere& two, CollisionData* data)
{
    // Make sure we have contacts
    if (data->mContactsLeft <= 0) return 0;

    // Cache the sphere positions
    Vector3 positionOne = one.GetAxis(3);
    Vector3 positionTwo = two.GetAxis(3);

    // Find the vector between the objects
    Vector3 midline = positionOne - positionTwo;
    real size = midline.Magnitude();

    // See if it is large enough
    if (size <= 0.0f || size >= one.mRadius + two.mRadius)
    {
        return 0;
    }

    // We manually create the normal, because we have the
    // size to hand
    Vector3 normal = midline * (((real)1.0) / size);

    Contact* contact		= data->mContacts;
    contact->mContactNormal = normal;
    contact->mContactPoint	= positionOne + midline * (real)0.5;
    contact->mPenetration	= (one.mRadius + two.mRadius - size);
    contact->SetBodyData(one.GetBody(), two.GetBody(), data->mFriction, data->mRestitution);

    data->AddContacts(1);

    return 1;
}

static inline real TransformToAxis(const CollisionBox& box, const Vector3& axis)
{
	return
		box.mHalfSize.x * real_abs(axis * box.GetAxis(0)) +
		box.mHalfSize.y * real_abs(axis * box.GetAxis(1)) +
		box.mHalfSize.z * real_abs(axis * box.GetAxis(2));
}

static inline real PenetrationOnAxis(const CollisionBox& one, const CollisionBox& two, const Vector3& axis, const Vector3& toCentre)
{
    // Project the half-size of one onto axis
    real oneProject = TransformToAxis(one, axis);
    real twoProject = TransformToAxis(two, axis);

    // Project this onto the axis
    real distance = real_abs(toCentre * axis);

    // Return the overlap (i.e. positive indicates
    // overlap, negative indicates separation).
    return oneProject + twoProject - distance;
}

static inline bool TryAxis(
    const CollisionBox& one,
    const CollisionBox& two,
    Vector3&			axis,
    const Vector3&		toCentre,
    unsigned			index,

    // These values may be updated
    real&				smallestPenetration,
    unsigned&			smallestCase)
{
    // Make sure we have a normalized axis, and don't check almost parallel axes
    if (axis.SquareMagnitude() < 0.0001) return true;
    axis.Normalise();

    real penetration = PenetrationOnAxis(one, two, axis, toCentre);

    if (penetration < 0) return false;
    if (penetration < smallestPenetration) 
	{
        smallestPenetration = penetration;
        smallestCase		= index;
    }

    return true;
}

void FillPointFaceBoxBox(
    const CollisionBox&	one,
    const CollisionBox&	two,
    const Vector3&		toCentre,
    CollisionData*		data,
    unsigned			best,
    real				pen)
{
    // This method is called when we know that a vertex from
    // box two is in contact with box one

    Contact* contact = data->mContacts;

    // We know which axis the collision is on (i.e. best),
    // but we need to work out which of the two faces on
    // this axis
    Vector3 normal = one.GetAxis(best);
    if (one.GetAxis(best) * toCentre > 0)
    {
        normal = normal * -1.0f;
    }

    // Work out which vertex of box two we're colliding with.
    // Using toCentre doesn't work!
    Vector3 vertex = two.mHalfSize;
    if (two.GetAxis(0) * normal < 0) vertex.x = -vertex.x;
    if (two.GetAxis(1) * normal < 0) vertex.y = -vertex.y;
    if (two.GetAxis(2) * normal < 0) vertex.z = -vertex.z;

    // Create the contact data
    contact->mContactNormal	= normal;
    contact->mPenetration	= pen;
    contact->mContactPoint	= two.GetTransform() * vertex;
    contact->SetBodyData(one.GetBody(), two.GetBody(), data->mFriction, data->mRestitution);
}

static inline Vector3 ContactPoint(
    const Vector3&	pOne,
    const Vector3&	dOne,
    real			oneSize,
    const Vector3&	pTwo,
    const Vector3&	dTwo,
    real			twoSize,

    // If this is true, and the contact point is outside
    // the edge (in the case of an edge-face contact) then
    // we use one's midpoint, otherwise we use two's
    bool			useOne)
{
    Vector3 toSt, cOne, cTwo;
    real	dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
    real	denom, mua, mub;

    smOne = dOne.SquareMagnitude();
    smTwo = dTwo.SquareMagnitude();
    dpOneTwo = dTwo * dOne;

    toSt		= pOne - pTwo;
    dpStaOne	= dOne * toSt;
    dpStaTwo	= dTwo * toSt;

    denom		= smOne * smTwo - dpOneTwo * dpOneTwo;

    // Zero denominator indicates parrallel lines
    if (real_abs(denom) < 0.0001f) 
	{
        return useOne ? pOne : pTwo;
    }

    mua = (dpOneTwo * dpStaTwo - smTwo * dpStaOne) / denom;
    mub = (smOne * dpStaTwo - dpOneTwo * dpStaOne) / denom;

    // If either of the edges has the nearest point out
    // of bounds, then the edges aren't crossed, we have
    // an edge-face contact. Our point is on the edge, which
    // we know from the useOne parameter
    if (mua > oneSize || mua < -oneSize ||
        mub > twoSize || mub < -twoSize)
    {
        return useOne?pOne:pTwo;
    }
    else
    {
        cOne = pOne + dOne * mua;
        cTwo = pTwo + dTwo * mub;

        return cOne * 0.5 + cTwo * 0.5;
    }
}

unsigned CollisionDetector::BoxAndBox(const CollisionBox& one, const CollisionBox& two, CollisionData* data)
{
    //if (!IntersectionTests::boxAndBox(one, two)) return 0;

    // Find the vector between the two centres
    Vector3 toCentre = two.GetAxis(3) - one.GetAxis(3);

    // We start assuming there is no contact
    real pen		= REAL_MAX;
    unsigned best	= 0xffffff;

    // Now we check each axes, returning if it gives us
    // a separating axis, and keeping track of the axis with
    // the smallest penetration otherwise
	if (!TryAxis(one, two, one.GetAxis(0), toCentre, 0, pen, best)) return 0;
	if (!TryAxis(one, two, one.GetAxis(1), toCentre, 1, pen, best)) return 0;
	if (!TryAxis(one, two, one.GetAxis(2), toCentre, 2, pen, best)) return 0;

	if (!TryAxis(one, two, two.GetAxis(0), toCentre, 3, pen, best)) return 0;
	if (!TryAxis(one, two, two.GetAxis(1), toCentre, 4, pen, best)) return 0;
	if (!TryAxis(one, two, two.GetAxis(2), toCentre, 5, pen, best)) return 0;

    // Store the best axis-major, in case we run into almost
    // parallel edge collisions later
    unsigned bestSingleAxis = best;

	if (!TryAxis(one, two, one.GetAxis(0) % two.GetAxis(0), toCentre, 6, pen, best))	return 0;
	if (!TryAxis(one, two, one.GetAxis(0) % two.GetAxis(1), toCentre, 7, pen, best))	return 0;
	if (!TryAxis(one, two, one.GetAxis(0) % two.GetAxis(2), toCentre, 8, pen, best))	return 0;
	if (!TryAxis(one, two, one.GetAxis(1) % two.GetAxis(0), toCentre, 9, pen, best))	return 0;
	if (!TryAxis(one, two, one.GetAxis(1) % two.GetAxis(1), toCentre, 10, pen, best))	return 0;
	if (!TryAxis(one, two, one.GetAxis(1) % two.GetAxis(2), toCentre, 11, pen, best))	return 0;
	if (!TryAxis(one, two, one.GetAxis(2) % two.GetAxis(0), toCentre, 12, pen, best))	return 0;
	if (!TryAxis(one, two, one.GetAxis(2) % two.GetAxis(1), toCentre, 13, pen, best))	return 0;
	if (!TryAxis(one, two, one.GetAxis(2) % two.GetAxis(2), toCentre, 14, pen, best))	return 0;

    // Make sure we've got a result
    assert(best != 0xffffff);

    // We now know there's a collision, and we know which
    // of the axes gave the smallest penetration. We now
    // can deal with it in different ways depending on
    // the case
    if (best < 3)
    {
        // We've got a vertex of box two on a face of box one
        FillPointFaceBoxBox(one, two, toCentre, data, best, pen);
        data->AddContacts(1);
        return 1;
    }
    else if (best < 6)
    {
        // We've got a vertex of box one on a face of box two.
        // We use the same algorithm as above, but swap around
        // one and two
        FillPointFaceBoxBox(two, one, toCentre * -1.0f, data, best - 3, pen);
        data->AddContacts(1);
        return 1;
    }
    else
    {
        // We've got an edge-edge contact. Find out which axes
        best -= 6;
        unsigned oneAxisIndex = best / 3;
        unsigned twoAxisIndex = best % 3;
        Vector3 oneAxis = one.GetAxis(oneAxisIndex);
        Vector3 twoAxis = two.GetAxis(twoAxisIndex);
        Vector3 axis = oneAxis % twoAxis;
        axis.Normalise();

        // The axis should point from box one to box two
        if (axis * toCentre > 0) axis = axis * -1.0f;

        // We have the axes, but not the edges: each axis has 4 edges parallel
        // to it, we need to find which of the 4 for each object. We do
        // that by finding the point in the centre of the edge. We know
        // its component in the direction of the box's collision axis is zero
        // (its a mid-point) and we determine which of the extremes in each
        // of the other axes is closest
        Vector3 ptOnOneEdge = one.mHalfSize;
        Vector3 ptOnTwoEdge = two.mHalfSize;
        for (unsigned i = 0; i < 3; ++i)
        {
            if (i == oneAxisIndex) ptOnOneEdge[i] = 0;
            else if (one.GetAxis(i) * axis > 0) ptOnOneEdge[i] = -ptOnOneEdge[i];

            if (i == twoAxisIndex) ptOnTwoEdge[i] = 0;
            else if (two.GetAxis(i) * axis < 0) ptOnTwoEdge[i] = -ptOnTwoEdge[i];
        }

        // Move them into world coordinates (they are already oriented
        // correctly, since they have been derived from the axes)
        ptOnOneEdge = one.GetTransform() * ptOnOneEdge;
        ptOnTwoEdge = two.GetTransform() * ptOnTwoEdge;

        // So we have a point and a direction for the colliding edges.
        // We need to find out point of closest approach of the two
        // line-segments
        Vector3 vertex = ContactPoint(
            ptOnOneEdge, oneAxis, one.mHalfSize[oneAxisIndex],
            ptOnTwoEdge, twoAxis, two.mHalfSize[twoAxisIndex],
            bestSingleAxis > 2);

        // We can fill the contact
        Contact* contact		= data->mContacts;
        contact->mPenetration	= pen;
        contact->mContactNormal = axis;
        contact->mContactPoint	= vertex;
        contact->SetBodyData(one.GetBody(), two.GetBody(), data->mFriction, data->mRestitution);

        data->AddContacts(1);
        
		return 1;
    }

    return 0;
}

unsigned CollisionDetector::BoxAndSphere(const CollisionBox& box, const CollisionSphere& sphere, CollisionData* data)
{
	// Transform the centre of the sphere into box coordinates
	Vector3 centre = sphere.GetAxis(3);
	Vector3 relCentre = box.GetTransform().TransformInverse(centre);

	// Early out check to see if we can exclude the contact
	if (real_abs(relCentre.x) - sphere.mRadius > box.mHalfSize.x ||
		real_abs(relCentre.y) - sphere.mRadius > box.mHalfSize.y ||
		real_abs(relCentre.z) - sphere.mRadius > box.mHalfSize.z)
	{
		return 0;
	}

	Vector3 closestPt(0, 0, 0);
	real dist;

	// Clamp each coordinate to the box
	dist = relCentre.x;
	if (dist >	box.mHalfSize.x)	dist =	box.mHalfSize.x;
	if (dist < -box.mHalfSize.x)	dist = -box.mHalfSize.x;
	closestPt.x = dist;

	dist = relCentre.y;
	if (dist >	box.mHalfSize.y) dist =	 box.mHalfSize.y;
	if (dist < -box.mHalfSize.y) dist = -box.mHalfSize.y;
	closestPt.y = dist;

	dist = relCentre.z;
	if (dist >	box.mHalfSize.z) dist =	 box.mHalfSize.z;
	if (dist < -box.mHalfSize.z) dist = -box.mHalfSize.z;
	closestPt.z = dist;

	// Check we're in contact
	dist = (closestPt - relCentre).SquareMagnitude();
	if (dist > sphere.mRadius * sphere.mRadius) return 0;

	// Compile the contact
	Vector3 closestPtWorld = box.GetTransform().Transform(closestPt);

	Contact* contact		= data->mContacts;
	contact->mContactNormal = (closestPtWorld - centre);
	contact->mContactNormal.Normalise();
	contact->mContactPoint	= closestPtWorld;
	contact->mPenetration	= sphere.mRadius - real_sqrt(dist);
	contact->SetBodyData(box.GetBody(), sphere.GetBody(), data->mFriction, data->mRestitution);

	data->AddContacts(1);

	return 1;
}

unsigned CollisionDetector::SphereAndPlane(const CollisionSphere& sphere, const CollisionPlane& plane, CollisionData* data)
{
	// Make sure we have contacts
	if (data->mContactsLeft <= 0) return 0;

	// Cache the sphere position
	Vector3 position = sphere.GetAxis(3);

	// Find the distance from the plane
	real ballDistance =
		plane.mDirection * position -
		sphere.mRadius - plane.mOffset;

	if (ballDistance >= 0) return 0;

	// Create the contact - it has a normal in the plane direction
	Contact* contact		= data->mContacts;
	contact->mContactNormal = plane.mDirection;
	contact->mPenetration	= -ballDistance;
	contact->mContactPoint	= position - plane.mDirection * (ballDistance + sphere.mRadius);
	contact->SetBodyData(sphere.GetBody(), NULL, data->mFriction, data->mRestitution);

	data->AddContacts(1);

	return 1;
}

//unsigned CollisionDetector::SphereAndTruePlane(const CollisionSphere& sphere, const CollisionPlane& plane, CollisionData* data)
//{
//	// Make sure we have contacts
//	if (data->mContactsLeft <= 0) return 0;
//
//	// Cache the sphere position
//	Vector3 position = sphere.GetAxis(3);
//
//	// Find the distance from the plane
//	real centreDistance = plane.mDirection * position - plane.mOffset;
//
//	// Check if we're within radius
//	if (centreDistance * centreDistance > sphere.mRadius * sphere.mRadius)
//	{
//		return 0;
//	}
//
//	// Check which side of the plane we're on
//	Vector3 normal = plane.mDirection;
//	real penetration = -centreDistance;
//
//	if (centreDistance < 0)
//	{
//		normal *= -1;
//		penetration = -penetration;
//	}
//
//	penetration += sphere.mRadius;
//
//	// Create the contact - it has a normal in the plane direction
//	Contact* contact		= data->mContacts;
//	contact->mContactNormal = normal;
//	contact->mPenetration	= penetration;
//	contact->mContactPoint	= position - plane.mDirection * centreDistance;
//	contact->SetBodyData(sphere.mBody, NULL, data->mFriction, data->mRestitution);
//
//	data->AddContacts(1);
//
//	return 1;
//}

unsigned CollisionDetector::BoxAndPlane(const CollisionBox& box, const CollisionPlane& plane, CollisionData* data)
{
    // Make sure we have contacts
    if (data->mContactsLeft <= 0) return 0;

    // Check for intersection
    if (!IntersectionTests::BoxAndPlane(box, plane))
    {
        return 0;
    }

    // We have an intersection, so find the intersection points. We can make
    // do with only checking vertices. If the box is resting on a plane
    // or on an edge, it will be reported as four or two contact points

    // Go through each combination of + and - for each half-size
    static real mults[8][3] = {	{1, 1, 1},	{-1, 1, 1},	{1,-1, 1},	{-1,-1, 1},
								{1, 1,-1},	{-1, 1,-1},	{1,-1,-1},	{-1,-1,-1}};

    Contact* contact = data->mContacts;
    unsigned contactsUsed = 0;
    for (unsigned i = 0; i < 8; ++i) 
	{
        // Calculate the position of each vertex
        Vector3 vertexPos(mults[i][0], mults[i][1], mults[i][2]);
        vertexPos.ComponentProductUpdate(box.mHalfSize);
        vertexPos = box.GetTransform().Transform(vertexPos);

        // Calculate the distance from the plane
        real vertexDistance = vertexPos * plane.mDirection;

        // Compare this to the plane's distance
        if (vertexDistance <= plane.mOffset)
        {
            // Create the contact data

            // The contact point is halfway between the vertex and the
            // plane - we multiply the direction by half the separation
            // distance and add the vertex location
            contact->mContactPoint	= plane.mDirection;
            contact->mContactPoint *= (vertexDistance - plane.mOffset);
            contact->mContactPoint	= vertexPos;
            contact->mContactNormal = plane.mDirection;
            contact->mPenetration	= plane.mOffset - vertexDistance;

            // Write the appropriate data
            contact->SetBodyData(box.GetBody(), NULL, data->mFriction, data->mRestitution);

            // Move onto the next contact
            ++contact;
            ++contactsUsed;

            if (contactsUsed == data->mContactsLeft) return contactsUsed;
        }
    }

    data->AddContacts(contactsUsed);

    return contactsUsed;
}

unsigned CollisionDetector::BoxAndPoint(const CollisionBox& box, const Vector3& point, CollisionData* data)
{
	// Transform the point into box coordinates
	Vector3 relPt = box.GetTransform().TransformInverse(point);

	Vector3 normal;

	// Check each axis, looking for the axis on which the
	// penetration is least deep
	real minDepth = box.mHalfSize.x - real_abs(relPt.x);

	if (minDepth < 0) return 0;

	normal = box.GetAxis(0) * (real)((relPt.x < 0) ? -1 : 1);

	real depth = box.mHalfSize.y - real_abs(relPt.y);

	if (depth < 0) return 0;
	else if (depth < minDepth)
	{
		minDepth	= depth;
		normal		= box.GetAxis(1) * (real)((relPt.y < 0) ? -1 : 1);
	}

	depth = box.mHalfSize.z - real_abs(relPt.z);

	if (depth < 0) return 0;
	else if (depth < minDepth)
	{
		minDepth	= depth;
		normal		= box.GetAxis(2) * (real)((relPt.z < 0) ? -1 : 1);
	}

	// Compile the contact
	Contact* contact		= data->mContacts;
	contact->mContactNormal = normal;
	contact->mContactPoint	= point;
	contact->mPenetration	= minDepth;

	// Note that we don't know what rigid body the point
	// belongs to, so we just use NULL. Where this is called
	// this value can be left, or filled in
	contact->SetBodyData(box.GetBody(), NULL, data->mFriction, data->mRestitution);

	data->AddContacts(1);

	return 1;
}

real XMatrixWorldTransformation(const int x, const real halfWidth, const real scale)
{
	real xWorld = (x - halfWidth) * scale;
	return xWorld;
}

real YMatrixWorldTransformation(const int y, const real halfHeight, const real scale) 
{
	real yWorld = (y - halfHeight) * scale;
	yWorld *= -1;
	return yWorld;
}

void CollideTriangleAndSphere(const Triangle& triangle, const CollisionSphere& sphere, const Vector3& sphereCenterProjection, CollisionData* data)
{
	Contact* contact = data->mContacts;
	contact->mContactNormal = triangle.GetNormal();

	Vector3 temp = contact->mContactNormal;
	sphere.GetBody()->GetPosition(&(contact->mContactPoint));
	temp.Invert();
	contact->mContactPoint += temp * sphere.mRadius;	

	Vector3 distance;
	sphere.GetBody()->GetPosition(&distance);
	distance -= sphereCenterProjection;
	contact->mPenetration = sphere.mRadius - distance.Magnitude();

	// TODO: Modify this call using triangle vertexes' materials.
	contact->SetBodyData(sphere.GetBody(), NULL, data->mFriction, data->mRestitution);

	data->AddContacts(1);
}

bool IntersectTriangleAndSphere(const Triangle& triangle, const CollisionSphere& sphere, Vector3& projection, CollisionData* data)
{
	//Without CCD

	bool isColliding = false;

	Vector3 sphereCenterProjection = triangle.PointProjectionOnPlane(sphere.GetBody()->GetPosition());

	if (triangle.IsProjectionInTriangle(sphereCenterProjection))
	{
		real distance = (sphere.GetBody()->GetPosition() - sphereCenterProjection).Magnitude();
		if (distance <= sphere.mRadius)
		{
			CollideTriangleAndSphere(triangle, sphere, sphereCenterProjection, data);

			isColliding = true;
		}
		//else isColliding = false;
	}
	//else return false;

	//With CCD
	if (!isColliding)
	{
		Vector3 directionMove = sphere.GetBody()->GetVelocity();
		if(directionMove.Magnitude()!=0)
		{
			//collisionPointOnSphere = D
			Vector3 collisionPointOnSphere = sphere.GetBody()->GetPosition() - (triangle.GetNormal() *  sphere.mRadius);

			Vector3 pointOnPlane = triangle.GetVertex0();
			Vector3 normalPlane = triangle.GetNormal();


			real a0 = 
				normalPlane.x * pointOnPlane.x +
				normalPlane.y * pointOnPlane.y +
				normalPlane.z * pointOnPlane.z;

			real a1 = 
				normalPlane.x * collisionPointOnSphere.x +
				normalPlane.y * collisionPointOnSphere.y +
				normalPlane.z * collisionPointOnSphere.z;

			real b0 = 
				normalPlane.x * directionMove.x +
				normalPlane.y * directionMove.y +
				normalPlane.z * directionMove.z;

			real timeStep = (a1 - a0) / b0;

			Vector3 projectionContactOnPlane = collisionPointOnSphere + directionMove * timeStep;
			if (timeStep >= 0 && timeStep <= 1 && triangle.IsProjectionInTriangle(projectionContactOnPlane))
			{
				//sphere->GetBody()->Integrate(timeStep);

				Contact* contact = data->mContacts;
				contact->mContactNormal = normalPlane;

				contact->mContactPoint = collisionPointOnSphere;	
				contact->mPenetration = 0.f;

				// TODO: Modify this call using triangle vertexes' materials.
				contact->SetBodyData(sphere.GetBody(), NULL, data->mFriction, data->mRestitution);

				data->AddContacts(1);

				isColliding = true;
			}
			else
				isColliding = false;
		}
		else
			isColliding = false;
	}

	return isColliding;
}

int TrianglesStarShape(const CollisionTerrain& terrain, const int i, const int j, const CollisionSphere& sphere, CollisionData* data)
{
	Triangle t0;
	Triangle t1;
	Triangle t2;
	Triangle t3;
	Triangle t4;
	Triangle t5;
	Triangle t6;
	Triangle t7;

	unsigned collisionCounter = 0;

	Vector3 vertex0;
	Vector3 vertex1;
	Vector3 vertex2;

	Material material0;
	Material material1;
	Material material2;

	Vector3 projection;

	const int r = (int)terrain.GetRows();
	const int c = (int)terrain.GetColumns();

	float worldHalfWidth = (c - 1) / 2.0f;
	float worldHalfHeight = (r - 1) / 2.0f;

	real horizontalScaleFactor = terrain.GetHorizontalScaleFactor();

	vertex0 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
						YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
						terrain.GetHeight(i, j));
	material0 = terrain.GetMaterial(i, j);

	if (i - 1 >= 0)
	{
		if (j - 1 >= 0)
		{
			vertex1 = Vector3(	XMatrixWorldTransformation(j - 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j - 1));
			material1 = terrain.GetMaterial(i, j - 1);

			vertex2 = Vector3(	XMatrixWorldTransformation(j - 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i - 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i - 1, j - 1));
			material2 = terrain.GetMaterial(i - 1, j - 1);

			t0 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			vertex1 = vertex2;
			material1 = material2;

			vertex2 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i - 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i - 1, j));
			material2 = terrain.GetMaterial(i - 1, j);

			t1 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			if (IntersectTriangleAndSphere(t0, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t0, sphere, projection, data);
			}

			if (IntersectTriangleAndSphere(t1, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t1, sphere, projection, data);
			}
		}

		if (j + 1 < c)
		{
			vertex1 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i - 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i - 1, j));
			material1 = terrain.GetMaterial(i - 1, j);

			vertex2 = Vector3(	XMatrixWorldTransformation(j + 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i - 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i - 1, j + 1));
			material2 = terrain.GetMaterial(i - 1, j);

			t2 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			vertex1 = vertex2;
			material1 = material2;

			vertex2 = Vector3(	XMatrixWorldTransformation(j + 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j + 1));
			material2 = terrain.GetMaterial(i, j + 1);
				
			t3 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			if (IntersectTriangleAndSphere(t2, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t2, sphere, projection, data);
			}

			if (IntersectTriangleAndSphere(t3, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t3, sphere, projection, data);
			}
		}
	}

	if (i + 1 < r)
	{
		if (j - 1 >= 0)
		{
			vertex1 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i + 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i + 1, j));
			material1 = terrain.GetMaterial(i + 1, j);

			vertex2 = Vector3(	XMatrixWorldTransformation(j - 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i + 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i + 1, j - 1));
			material2 = terrain.GetMaterial(i + 1, j - 1);
		
			t6 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			vertex1 = vertex2;
			material1 = material2;

			vertex2 = Vector3(	XMatrixWorldTransformation(j - 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j - 1));
			material2 = terrain.GetMaterial(i, j - 1);

			t7 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);
			
			if (IntersectTriangleAndSphere(t6, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t6, sphere, projection, data);
			}

			if (IntersectTriangleAndSphere(t7, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t7, sphere, projection, data);
			}
		}

		if (j + 1 < c)
		{
			vertex1 = Vector3(	XMatrixWorldTransformation(j + 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j + 1));
			material1 = terrain.GetMaterial(i, j + 1);

			vertex2 = Vector3(	XMatrixWorldTransformation(j + 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i + 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i + 1, j + 1));
			material2 = terrain.GetMaterial(i + 1, j + 1);

			t4 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			vertex1 = vertex2;
			material1 = material2;

			vertex2 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i + 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i + 1, j));
			material2 = terrain.GetMaterial(i + 1, j);

			t5 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			if (IntersectTriangleAndSphere(t4, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t4, sphere, projection, data);
			}

			if (IntersectTriangleAndSphere(t5, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t5, sphere, projection, data);
			}
		}
	}

	return collisionCounter;
}

int TrianglesKiteShape(const CollisionTerrain& terrain, const int i, const int j, const CollisionSphere& sphere, CollisionData* data)
{
	Triangle t0;
	Triangle t1;
	Triangle t2;
	Triangle t3;
	Triangle t4;
	Triangle t5;
	Triangle t6;
	Triangle t7;

	int collisionCounter = 0;

	Vector3 vertex0;
	Vector3 vertex1;
	Vector3 vertex2;

	Material material0;
	Material material1;
	Material material2;

	Vector3 projection;

	const int r = (int)terrain.GetRows();
	const int c = (int)terrain.GetColumns();

	float worldHalfWidth = (c - 1) / 2.0f;
	float worldHalfHeight = (r - 1) / 2.0f;

	real horizontalScaleFactor = terrain.GetHorizontalScaleFactor();

	if (i - 1 >= 0)
	{
		if (j - 1 >= 0)
		{
			vertex0 = Vector3(	XMatrixWorldTransformation(j - 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i - 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i - 1, j - 1));
			material0 = terrain.GetMaterial(i - 1, j - 1);

			vertex1 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i - 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i - 1, j));
			material1 = terrain.GetMaterial(i - 1, j);

			vertex2 = Vector3(	XMatrixWorldTransformation(j - 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j - 1));
			material2 = terrain.GetMaterial(i, j - 1);

			t0 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			vertex0 = vertex1;
			material0 = material1;

			vertex1 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j));
			material1 = terrain.GetMaterial(i, j);

			t1 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);
			 
			if (IntersectTriangleAndSphere(t0, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t0, sphere, projection, data);
			}

			if (IntersectTriangleAndSphere(t1, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t1, sphere, projection, data);
			}
		}

		if (j + 1 < c)
		{
			vertex0 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i - 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i - 1, j));
			material0 = terrain.GetMaterial(i - 1, j);

			vertex1 = Vector3(	XMatrixWorldTransformation(j + 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j + 1));
			material1 = terrain.GetMaterial(i, j + 1);

			vertex2 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j));
			material2 = terrain.GetMaterial(i, j);

			t2 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			vertex1 = Vector3(	XMatrixWorldTransformation(j + 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i - 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i - 1, j + 1));
			material1 = terrain.GetMaterial(i - 1, j + 1);
			
			vertex2 = vertex1;
			material2 = material1;

			t3 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			if (IntersectTriangleAndSphere(t2, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t2, sphere, projection, data);
			}

			if (IntersectTriangleAndSphere(t3, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t3, sphere, projection, data);
			}
		}
	}

	if (i + 1 < r)
	{
		if (j - 1 >= 0)
		{
			vertex0 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i + 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i + 1, j));
			material0 = terrain.GetMaterial(i + 1, j);

			vertex1 = Vector3(	XMatrixWorldTransformation(j - 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j - 1));
			material1 = terrain.GetMaterial(i, j - 1);

			vertex2 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j));
			material2 = terrain.GetMaterial(i, j);

			t6 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			vertex1 = Vector3(	XMatrixWorldTransformation(j - 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i + 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i + 1, j - 1));
			material1 = terrain.GetMaterial(i + 1, j - 1);

			vertex2 = vertex1;
			material2 = material1;

			t7 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			if (IntersectTriangleAndSphere(t6, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t6, sphere, projection, data);
			}

			if (IntersectTriangleAndSphere(t7, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t7, sphere, projection, data);
			}
		}

		if (j + 1 < c)
		{
			vertex0 = Vector3(	XMatrixWorldTransformation(j + 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j + 1));
			material0 = terrain.GetMaterial(i, j + 1);

			vertex1 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i + 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i + 1, j));
			material1 = terrain.GetMaterial(i + 1, j);

			vertex2 = Vector3(	XMatrixWorldTransformation(j, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i, j));
			material2 = terrain.GetMaterial(i, j);

			t4 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			vertex1 = Vector3(	XMatrixWorldTransformation(j + 1, worldHalfWidth, horizontalScaleFactor), 
								YMatrixWorldTransformation(i + 1, worldHalfHeight, horizontalScaleFactor), 
								terrain.GetHeight(i + 1, j + 1));
			material1 = terrain.GetMaterial(i + 1, j + 1);

			vertex2 = vertex1;
			material2 = material1;

			t5 = Triangle(vertex0, vertex1, vertex2,
				material0, material1, material2);

			if (IntersectTriangleAndSphere(t4, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t4, sphere, projection, data);
			}

			if (IntersectTriangleAndSphere(t5, sphere, projection, data))
			{
				++collisionCounter;
				//CollideTriangleAndSphere(t5, sphere, projection, data);
			}
		}
	}

	return collisionCounter;
}

unsigned CollisionDetector::TerrainAndSphere(const CollisionTerrain& terrain, const CollisionSphere& sphere, CollisionData* data)
{
	const int r = (int)terrain.GetRows();
	const int c = (int)terrain.GetColumns();

	float worldHalfWidth = (c - 1) / 2.0f;
	float worldHalfHeight = (r - 1) / 2.0f;

	real worldToMatrixX = sphere.GetBody()->GetPosition().x;
	worldToMatrixX /= terrain.GetHorizontalScaleFactor();
	worldToMatrixX += worldHalfWidth;

	real worldToMatrixY = sphere.GetBody()->GetPosition().y * -1;
	worldToMatrixY /= terrain.GetHorizontalScaleFactor();
	worldToMatrixY += worldHalfHeight;

	int j = PhxUtils::RoundReal(worldToMatrixX);
	int i = PhxUtils::RoundReal(worldToMatrixY);

	unsigned collisionCounter = 0;

	if (0 <= i && 0 <= j && r > i && c > j)
	{
		if ((i + j) % 2 == 0)
		{
			// Star shape.
			collisionCounter = TrianglesStarShape(terrain, i, j, sphere, data);
		}
		else
		{
			// Quad shape.
			collisionCounter = TrianglesKiteShape(terrain, i, j, sphere, data);			
		}
	}

	return collisionCounter;
}
